#include "packet.h"
#include <iostream>

void Packet::buildPacket(uint8_t src_addr, uint8_t dst_addr, uint8_t cmd, 
                        const void* payload, uint16_t payload_len) {
    data_.clear();
    
    // 帧头 (0x55AA)
    data_.push_back(0x55);
    data_.push_back(0xAA);
    
    // 源地址
    data_.push_back(src_addr);
    
    // 目标地址
    data_.push_back(dst_addr);
    
    // 命令字
    data_.push_back(cmd);
    
    // 数据长度（小端）
    data_.push_back(payload_len & 0xFF);        // L
    data_.push_back((payload_len >> 8) & 0xFF); // H
    
    // 数据段
    if (payload && payload_len > 0) {
        const uint8_t* payload_bytes = static_cast<const uint8_t*>(payload);
        data_.insert(data_.end(), payload_bytes, payload_bytes + payload_len);
    }
    
    // CRC校验（除CRC外的所有数据）
    uint16_t crc = calculateCRC16(data_.data(), data_.size());
    data_.push_back(crc & 0xFF);        // L
    data_.push_back((crc >> 8) & 0xFF); // H
}

bool Packet::parsePacket(const std::vector<uint8_t>& raw_data, 
                        uint8_t& src_addr, uint8_t& dst_addr, uint8_t& cmd,
                        std::vector<uint8_t>& payload) {
    if (raw_data.size() < 9) { // 最小包长度
        return false;
    }
    
    // 检查帧头
    if (raw_data[0] != 0x55 || raw_data[1] != 0xAA) {
        return false;
    }
    
    // 解析头部
    src_addr = raw_data[2];
    dst_addr = raw_data[3];
    cmd = raw_data[4];
    
    // 解析数据长度（小端）
    uint16_t payload_len = raw_data[5] | (raw_data[6] << 8);
    
    // 检查包长度
    if (raw_data.size() != 7 + payload_len + 2) {
        return false;
    }
    
    // 验证CRC
    uint16_t received_crc = raw_data[raw_data.size()-2] | 
                           (raw_data[raw_data.size()-1] << 8);
    uint16_t calculated_crc = calculateCRC16(raw_data.data(), raw_data.size()-2);
    
    if (received_crc != calculated_crc) {
        std::cout << "CRC校验失败" << std::endl;
        return false;
    }
    
    // 提取payload
    payload.clear();
    if (payload_len > 0) {
        payload.assign(raw_data.begin() + 7, raw_data.begin() + 7 + payload_len);
    }
    
    return true;
}

uint16_t Packet::readUint16LE(const uint8_t* data) const {
    return data[0] | (data[1] << 8);
}

void Packet::writeUint16LE(uint16_t value) {
    data_.push_back(value & 0xFF);
    data_.push_back((value >> 8) & 0xFF);
}